#include <Servo.h>

#define N_SERVOS 18
Servo servos[N_SERVOS];

const int servo_pin[N_SERVOS] = {
  35, 36, 37,
  27, 28, 29,
  24, 25, 26,

  38, 39, 40,
  46, 47, 48,
  49, 50, 51
};
const int servo_pos[N_SERVOS] = {
  deg2ms(90), deg2ms(0), deg2ms(0),
  deg2ms(90), deg2ms(0), deg2ms(0),
  deg2ms(90), deg2ms(0), deg2ms(0),

  deg2ms(90), deg2ms(180), deg2ms(180),
  deg2ms(90), deg2ms(180), deg2ms(180),
  deg2ms(90), deg2ms(180), deg2ms(180),
};
const int servo_cal[3][N_SERVOS] = {
  { // low 0°
    0,  0,  0,
    0,  0,  0,
    0,  0,  0,

    0,  0,  0,
    0,  0,  0,
    0,  0,  0
  }
  ,
  { // medium 90°
    25,  0,  35,
    65,  -45,  45,
    -45,  -40,  35,

    50,  55,  -25,
    -20,  55,  -25,
    -5,  55,  -25
  }
  ,
  { // high 180°
    0,  0,  0,
    0,  0,  0,
    0,  0,  0,

    0,  0,  0,
    0,  0,  0,
    0,  0,  0
  }
};
boolean servo_update[N_SERVOS] = {
  true,  true,  true,
  true,  true,  true,
  true,  true,  true,

  true,  true,  true,
  true,  true,  true,
  true,  true,  true,
};

boolean g_isReady = false;
boolean g_isConnected = false;
unsigned long g_time=0;
boolean g_isDebug = false;
void setup()
{
  Serial.begin(9600); // standard output ^^

  Serial.println("Initialize servomotors...");

  init_servos();
  set_pack_position();
  update_servos();
  delay(2500);

  Serial.println("Wait commands...");
}

void loop()
{
  // got direct seriel commands
  check_basic_commands(); // to slow !

  // update servo positions
  update_servos();
}



